from CRC import * #import the Checksum functions
import time



    
def sendCommand(Forward,Turn):
    
    if Forward < 0:
        Forward = 0xFFFF + Forward + 1
    if Turn < 0:
        Turn = 0xFFFF + Turn + 1

#Make up the command string starting with M for the motion command
#Then the Event ID i.e 0 for stridge length 1 for stride height 2 for swing etc
#Then the value to be send and finally a set of trailing zeros
    sendstring = "H"+"%0.4X"%Forward+"%0.4X"%Turn+"0000"

    addCRCandSend(sendstring)

def getIMUdata():
    
    sendstring = "I"+"0078"+"0000"+"0000"
    addCRCandSend(sendstring)
    #
    line = readDATA()

    try:
        data = [int(x) for x in line.split()]
    except:
        print "no valid data read", line
        data = []
        
    return data

def getServos():

    sendstring = "K"+"0000"+"0000"+"0000"
    addCRCandSend(sendstring)
    #
    line = readDATA()

    try:
        data = [int(x) for x in line.split()]
    except:
        print "no valid data read", line
        data = []

    return data

def setServo( id, pos):

    sendstring = "A"+"%0.4X"%id+"%0.4X"%pos+"00FF"
    print sendstring
    addCRCandSend(sendstring)
